#include "imu.h"
#include "mpu6050.h"

eulerAngle RoboAngle;

void IMU_Update(float half_T, vec3f* gyro, vec3f* acc)
{
    float vx, vy, vz;//(r系到b系的第三列)

    float norm;
    float ex, ey, ez;

    float gx = gyro->data[x];
    float gy = gyro->data[y];
    float gz = gyro->data[z];
    float ax = acc->data[x];
    float ay = acc->data[y];
    float az = acc->data[z];

    static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;     // quaternion elements representing the estimated orientation
    static float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error

    //acc数据归一化
    norm = my_sqrt(ax*ax + ay*ay + az*az);
    ax = ax / norm;
    ay = ay / norm;
    az = az / norm;

    // estimated direction of gravity and flux (v and w)             
    //四元素中xyz的表示, 估计重力方向和流量/变迁
    vx = 2*(q1*q3 - q0*q2);												
    vy = 2*(q0*q1 + q2*q3);
    vz = 1 - 2*(q1*q1 + q2*q2);

    // error is sum of cross product between reference direction of fields and direction measured by sensors
    //向量外积在相减得到差分就是误差
    ex = (ay*vz - az*vy) ;                           					 
    ey = (az*vx - ax*vz) ;
    ez = (ax*vy - ay*vx) ;

    //对误差进行积分
    exInt = exInt + ex *Ki *2 *half_T;  
    eyInt = eyInt + ey *Ki *2 *half_T;
    ezInt = ezInt + ez *Ki *2 *half_T;

    // 积分限幅
    exInt = LIMIT(exInt, - IMU_INTEGRAL_LIM,IMU_INTEGRAL_LIM );
    exInt = LIMIT(exInt, - IMU_INTEGRAL_LIM,IMU_INTEGRAL_LIM );
    exInt = LIMIT(exInt, - IMU_INTEGRAL_LIM,IMU_INTEGRAL_LIM );

    // adjusted gyroscope measurements
    gx = gx + Kp *(ex + exInt);
    gy = gy + Kp *(ey + eyInt);
    gz = gz + Kp *(ez + ezInt);

    // integrate quaternion rate and normalise
    //四元素的微分方程
    q0 = q0 + (-q1*gx - q2*gy - q3*gz) *half_T;
    q1 = q1 + ( q0*gx + q2*gz - q3*gy) *half_T;
    q2 = q2 + ( q0*gy - q1*gz + q3*gx) *half_T;
    q3 = q3 + ( q0*gz + q1*gy - q2*gx) *half_T;

    // normalise quaternion
    norm = my_sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;

    RoboAngle.yaw = fast_atan2(2*q1*q2+2*q0*q3, -2*q2*q2-2*q3*q3+1) *57.3f;
    RoboAngle.roll = fast_atan2(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1) *57.3f;
    RoboAngle.pitch = asin(-2*q1*q3 + 2*q0*q2) *57.3f;
}
